
// 引入必要的头文件
#include "../board/sdk_project_config.h"  // 包含开发板的配置信息
#include "../board/motor_control.h"       // 包含基本电机控制函数
#include "../board/dc_motor_control.h"    // 包含高级电机控制函数
#include "../ESWIN_SDK/log/include/log.h" // 包含日志功能
#include "../board/hcsr04.h"  // 包含超声波传感器函数
#include "move.h"
#include "pose.h"
#include "uarttouart.h"
#include "../board/map.h"
#include "board_delay.h"
extern volatile uint32_t uart5_rx_count;

#include <stdio.h>  // 标准输入输出库
#include <string.h> // 字符串处理库
#include <math.h>   // 数学库

// 定义延时函数 - 注释掉OS版本，改用简单延时
// #define DELAY_MS(ms) OS_DelayMs(ms)
#define DELAY_MS(ms) simple_delay_ms(ms)

// 定义时间参数
#define POWER_ON_DELAY_MS 1000  // 上电延时1秒(从5秒改为1秒加快测试)
#define MOVE_STRAIGHT_TIME_MS 8000  // 直行8秒
#define TURN_LEFT_TIME_MS 300     // 左转0.3秒
#define TURN_RIGHT_TIME_MS 300    // 右转0.3秒，转90度
#define MOTOR_TEST_TIME_MS 2000   // 每个电机测试时间，从3秒减为2秒

// 定义小车运动参数
#define STRAIGHT_SPEED 0.05f      // 直行速度，设置为0.1f
#define TURN_SPEED 0.1f          // 转弯速度，设置为0.1f
#define STRAIGHT_DISTANCE_TIME 3000  // 直行1米时间（3秒）
#define HALF_STRAIGHT_DISTANCE_TIME 1500  // 直行0.5米时间（1.5秒）

// UART5相关定义
#define UART5_INSTANCE INST_UART_5
#define UART5_RX_BUFFER_SIZE 1
#define UART5_TX_BUFFER_SIZE 64

// UART2相关定义
#define UART2_INSTANCE INST_UART_2

// 坐标结构体
// 删除 Position 结构体、g_position_map、MoveFromPositionToTarget 函数声明和实现

// 目的地位置
#define TARGET_POSITION 9  // 可以更改为其他位置（1-9）

// 函数声明
void EnableAllMotorDrivers(void);
void PrintMotorInfo(void);
void InitUART2(void);
void DriveForwardFor(uint32_t timeMs);
void TurnRight90Degrees(void);
void TurnLeft90Degrees(void);
void Turn180Degrees(void);
// 删除 MoveFromPositionToTarget 函数声明和实现
void ProcessUART5Command(uint8_t command);
void ServoDemo(void); // 新增舵机演示函数
void CheckObstacleAndStop(void);
bool g_bObstacleDetected = false; // 障碍物检测标志

// 新增：根据二维码邻接关系移动一步
void MoveOneStep(int from_id, int to_id) {
    const QRNode_t* node = &g_qr_map[from_id-1];
    if (node->up == to_id) {
        // 需要面向北，前进一格
        StopAllMotors();
        SetMotor1Direction(FORWARD); SetMotor1Speed(0x4000);
        SetMotor2Direction(FORWARD); SetMotor2Speed(0x4000);
        SetMotor3Direction(FORWARD); SetMotor3Speed(0x4000);
        SetMotor4Direction(FORWARD); SetMotor4Speed(0x4000);
        DELAY_MS(HALF_STRAIGHT_DISTANCE_TIME);
        StopAllMotors();
    } else if (node->down == to_id) {
        // 需要面向南
        Turn180Degrees();
        StopAllMotors();
        SetMotor1Direction(FORWARD); SetMotor1Speed(0x4000);
        SetMotor2Direction(FORWARD); SetMotor2Speed(0x4000);
        SetMotor3Direction(FORWARD); SetMotor3Speed(0x4000);
        SetMotor4Direction(FORWARD); SetMotor4Speed(0x4000);
        DELAY_MS(HALF_STRAIGHT_DISTANCE_TIME);
        StopAllMotors();
    } else if (node->left == to_id) {
        // 需要面向西
        TurnLeft90Degrees();
        StopAllMotors();
        SetMotor1Direction(FORWARD); SetMotor1Speed(0x4000);
        SetMotor2Direction(FORWARD); SetMotor2Speed(0x4000);
        SetMotor3Direction(FORWARD); SetMotor3Speed(0x4000);
        SetMotor4Direction(FORWARD); SetMotor4Speed(0x4000);
        DELAY_MS(HALF_STRAIGHT_DISTANCE_TIME);
        StopAllMotors();
    } else if (node->right == to_id) {
        // 需要面向东
        TurnRight90Degrees();
        StopAllMotors();
        SetMotor1Direction(FORWARD); SetMotor1Speed(0x4000);
        SetMotor2Direction(FORWARD); SetMotor2Speed(0x4000);
        SetMotor3Direction(FORWARD); SetMotor3Speed(0x4000);
        SetMotor4Direction(FORWARD); SetMotor4Speed(0x4000);
        DELAY_MS(HALF_STRAIGHT_DISTANCE_TIME);
        StopAllMotors();
    } else {
        printf("MoveOneStep错误：%d和%d不相邻\r\n", from_id, to_id);
    }
}

// 新增：全局路径规划+逐步移动，每到一个二维码就姿态调整并重新规划
void MoveByMapAndPose(int start_id, int target_id) {
    int curr_id = start_id;
    while (curr_id != target_id) {
        int path[9];
        int path_len = Map_GetFullPath(curr_id, target_id, path, 9);
        if (path_len < 2) {
            printf("无法规划到目标或已到达\r\n");
            break;
        }
        int next_id = path[1];
        printf("从%d移动到%d\r\n", curr_id, next_id);
        MoveOneStep(curr_id, next_id);
        // 到达后进行姿态调整
        Pose_AdjustByK230UART5(2.0f, 2.0f, 3000);
        curr_id = next_id;
    }
    printf("已到达目标二维码%d\r\n", target_id);
}

/**
 * @brief 直接设置STBY引脚为高电平，确保电机驱动芯片被使能
 */
void EnableAllMotorDrivers(void)
{
    // 直接设置所有STBY引脚为高电平
    PINS_DRV_WritePin(MOTOR1_STBY_PORT, MOTOR1_STBY_PIN, 1);
    PINS_DRV_WritePin(MOTOR2_STBY_PORT, MOTOR2_STBY_PIN, 1);
    PINS_DRV_WritePin(MOTOR3_STBY_PORT, MOTOR3_STBY_PIN, 1);
    PINS_DRV_WritePin(MOTOR4_STBY_PORT, MOTOR4_STBY_PIN, 1);
    
    // 延时确保STBY有效
    DELAY_MS(10);
}

/**
 * @brief 显示电机速度和占空比信息
 * 该函数用于监控电机实际速度、参考速度和PWM占空比之间的关系，
 * 便于评估编码器闭环控制的效果
 */
void PrintMotorInfo(void)
{
    // 计算占空比百分比
    float duty_percent1 = (float)g_stPwm2Config0.duty / 65535.0f * 100.0f;
    float duty_percent2 = (float)g_stPwm0Config0.duty / 65535.0f * 100.0f;
    float duty_percent3 = (float)g_stPwm1Config0.duty / 65535.0f * 100.0f;
    float duty_percent4 = (float)g_stPwm3Config0.duty / 65535.0f * 100.0f;
    
    // 通过UART2输出占空比信息
    printf("==== Motor Duty Cycle Info ====\r\n");
    printf("Motor1(RR): %.1f%%  Motor2(RF): %.1f%%\r\n", duty_percent1, duty_percent2);
    printf("Motor3(LF): %.1f%%  Motor4(LR): %.1f%%\r\n", duty_percent3, duty_percent4);
    
    // 打印电机速度信息
    printf("==== Motor Speed Info ====\r\n");
    printf("Motor2(RF): Encoder:%u ActSpeed:%.2f RefSpeed:%.2f\r\n",
           g_au16EncoderCounts[1], g_af32MotorSpeeds[1], g_af32MotorSpeedRefs[1]);
    printf("Motor3(LF): Encoder:%u ActSpeed:%.2f RefSpeed:%.2f\r\n\r\n",
           g_au16EncoderCounts[2], g_af32MotorSpeeds[2], g_af32MotorSpeedRefs[2]);
}

/**
 * @brief 初始化UART2用于日志输出
 */
void InitUART2(void)
{
    // 初始化UART2
    UART_DRV_Init(UART2_INSTANCE, &g_stUartState_2, &g_stUart2UserConfig0);
    
    // 设置日志端口为UART2
    setLogPort(2);
    
    // 发送控制字符，设置终端模式为正确处理中文
    // 发送ESC序列，尝试设置终端为UTF-8模式
    UART_DRV_SendDataBlocking(UART2_INSTANCE, (const uint8_t*)"\x1B%G", 3, 500);
    
    // 延时一小段时间确保设置生效
    DELAY_MS(10);
    
    printf("\r\n\r\n=== Motor Control System Started ===\r\n");
    printf("UART2 initialized, ready to print motor duty cycle and speed info\r\n");
}

/**
 * @brief 直行指定时间
 * @param timeMs 行驶时间（毫秒）
 */
void DriveForwardFor(uint32_t timeMs)
{
    // 直行控制循环，增加障碍物检测
    StopAllMotors();
    SetMotor1Direction(FORWARD); SetMotor1Speed(0x4000);
    SetMotor2Direction(FORWARD); SetMotor2Speed(0x4000);
    SetMotor3Direction(FORWARD); SetMotor3Speed(0x4000);
    SetMotor4Direction(FORWARD); SetMotor4Speed(0x4000);
    for(int i = 0; i < timeMs/10; i++) {
        if(i % 10 == 0) {
            CheckObstacleAndStop();
            if(g_bObstacleDetected) {
                StopAllMotors();
                return;
            }
            PrintMotorInfo();
        }
        DELAY_MS(10);
    }
    StopAllMotors();
    DELAY_MS(300);
}

/**
 * @brief 原地右转90度
 */
void TurnRight90Degrees(void)
{
    // 右轮前进，左轮后退，原地右转
    StopAllMotors();
    SetMotor1Direction(FORWARD); SetMotor1Speed(0x4000);
    SetMotor2Direction(FORWARD); SetMotor2Speed(0x4000);
    SetMotor3Direction(BACKWARD); SetMotor3Speed(0x4000);
    SetMotor4Direction(BACKWARD); SetMotor4Speed(0x4000);
    DELAY_MS(TURN_RIGHT_TIME_MS);
    StopAllMotors();
    DELAY_MS(300);
}

/**
 * @brief 原地左转90度
 */
void TurnLeft90Degrees(void)
{
    // 左轮前进，右轮后退，原地左转
    StopAllMotors();
    SetMotor1Direction(BACKWARD); SetMotor1Speed(0x4000);
    SetMotor2Direction(BACKWARD); SetMotor2Speed(0x4000);
    SetMotor3Direction(FORWARD); SetMotor3Speed(0x4000);
    SetMotor4Direction(FORWARD); SetMotor4Speed(0x4000);
    DELAY_MS(TURN_LEFT_TIME_MS);
    StopAllMotors();
    DELAY_MS(300);
}

/**
 * @brief 原地转向180度
 */
void Turn180Degrees(void)
{
    // 原地右转180度
    StopAllMotors();
    SetMotor1Direction(FORWARD); SetMotor1Speed(0x4000);
    SetMotor2Direction(FORWARD); SetMotor2Speed(0x4000);
    SetMotor3Direction(BACKWARD); SetMotor3Speed(0x4000);
    SetMotor4Direction(BACKWARD); SetMotor4Speed(0x4000);
    DELAY_MS(TURN_RIGHT_TIME_MS * 2);
    StopAllMotors();
    DELAY_MS(300);
}

/**
 * @brief 舵机演示函数
 * 演示舵机从0度到180度的往返运动
 */
void ServoDemo(void)
{
    printf("开始舵机演示: 0->180->0度往返运动\r\n");
    
    // 0->180
    for (int angle = 0; angle <= 180; angle += 5) {
        servo_set_angle(angle);
        printf("舵机角度: %d度\r\n", angle);
        DELAY_MS(100);
    }
    
    // 180->0
    for (int angle = 180; angle >= 0; angle -= 5) {
        servo_set_angle(angle);
        printf("舵机角度: %d度\r\n", angle);
        DELAY_MS(100);
    }
    
    printf("舵机演示完成\r\n");
}

/**
 * @brief 舵机从0度转到180度
 */
void ServoMoveTo180(void) {
    for (int angle = 0; angle <= 180; angle += 5) {
        servo_set_angle(angle); // 设置舵机角度
        DELAY_MS(100); // 延时20毫秒
    }
}

/**
 * @brief 舵机从180度转到0度
 */
void ServoMoveTo0(void) {
    for (int angle = 180; angle >= 0; angle -= 5) {
        servo_set_angle(angle); // 设置舵机角度
        DELAY_MS(100); // 延时20毫秒
    }
}

/**
 * @brief 处理UART5接收到的命令
 */
void ProcessUART5Command(uint8_t command)
{
    if (command >= '1' && command <= '9') {
        int position = command - '0';
        if(HCSR04_IsObstacleDetected()) {
            return;
        }
        // 用全局路径+逐步移动方案
        MoveByMapAndPose(position, TARGET_POSITION);
        DELAY_MS(5000);
        MoveByMapAndPose(TARGET_POSITION, 6);
    } else if (command == 's' || command == 'S') {
        ServoDemo();
    } else if (command == 'd' || command == 'D') {
        // 新增距离测量命令
        float distance = HCSR04_MeasureDistance();
        if(distance > 0) {
            printf("当前距离: %.1f cm\r\n", distance);
        } else {
            printf("距离测量失败\r\n");
        }
    } else {
        printf("Unknown command. Available: 1-9, s(servo), d(distance)\r\n");
    }
}

/**
 * @brief 主函数
 */
int main(void)
{
    CLOCK_SYS_Init(g_pstClockManConfigsArr[0]);
    PINS_DRV_Init(NUM_OF_CONFIGURED_PINS, g_stPinmuxConfigArr);
    DCMotor_Init();
    EnableAllMotorDrivers();
    UART_DRV_Init(UART2_INSTANCE, &g_stUartState_2, &g_stUart2UserConfig0);
    UART_DRV_Init(UART5_INSTANCE, &g_stUartState_5, &g_stUart5UserConfig0);
    setLogPort(2);
    HCSR04_Init(); // 初始化超声波

    // ========== 新主流程 ========== 
    printf("\r\n小车启动，开始二维码导航流程\r\n");

    int curr_id = 0; // 当前二维码ID，假设上电后通过UART5获取
    char err_buffer[128];
    QR_ErrData_t qr_err;
    
    // 1. 上电后，等待并解析一次二维码误差数据，获取初始ID
    while (1) {
        int len = UART5_ReceiveERRData(err_buffer, sizeof(err_buffer), 3000);
        if (len > 0) {
            qr_err = Pose_ParseQRData(err_buffer);
            if (qr_err.valid) {
                curr_id = qr_err.id;
                printf("初始二维码ID: %d\r\n", curr_id);
                break;
            }
        }
        printf("等待二维码数据...\r\n");
        DELAY_MS(1000);
    }

    // 2. 初始姿态矫正
    Pose_AdjustByK230UART5(2.0f, 2.0f, 3000);

    // 3. 路径规划与逐步移动
    while (curr_id != TARGET_POSITION) {
        int path[9];
        int path_len = Map_GetFullPath(curr_id, TARGET_POSITION, path, 9);
        if (path_len < 2) {
            printf("无法规划到目标或已到达\r\n");
            break;
        }
        int next_id = path[1];
        printf("从%d移动到%d\r\n", curr_id, next_id);
        // === 新增：移动前避障 ===
        while (HCSR04_IsObstacleDetected()) {
            printf("前方有障碍物，等待清除...\r\n");
            StopAllMotors();
            DELAY_MS(500);
        }
        MoveOneStep(curr_id, next_id);
        // === 新增：移动后再次避障检测 ===
        if (HCSR04_IsObstacleDetected()) {
            printf("移动后检测到障碍物，已停车，等待障碍物消失...\r\n");
            StopAllMotors();
            while (HCSR04_IsObstacleDetected()) {
                DELAY_MS(500);
            }
            printf("障碍物已清除，继续流程\r\n");
        }
        // 到达新二维码后，进行姿态矫正
        Pose_AdjustByK230UART5(2.0f, 2.0f, 3000);
        // 再次获取当前二维码ID（通过UART5）
        int got_new_id = 0;
        for (int try = 0; try < 3; ++try) {
            int len = UART5_ReceiveERRData(err_buffer, sizeof(err_buffer), 2000);
            if (len > 0) {
                qr_err = Pose_ParseQRData(err_buffer);
                if (qr_err.valid) {
                    curr_id = qr_err.id;
                    got_new_id = 1;
                    break;
                }
            }
            DELAY_MS(500);
        }
        if (!got_new_id) {
            printf("无法获取新二维码ID，流程中断\r\n");
            break;
        }
    }

    // 4. 到达目标二维码后，最后一次姿态矫正
    printf("已到达目标二维码%d，进行最终姿态矫正\r\n", TARGET_POSITION);
    Pose_AdjustByK230UART5(2.0f, 2.0f, 3000);
    StopAllMotors();
    printf("小车已停止\r\n");

    // 5. 舵机往返运动
    printf("开始舵机往返运动演示\r\n");
    ServoDemo();
    printf("流程结束\r\n");

    // ========== 新增：前往新的二维码ID再进行一次舵机运动 ========== 
    #define TARGET_POSITION2 6  // 新的目标二维码ID，可根据需要修改
    printf("准备前往新的二维码ID %d\r\n", TARGET_POSITION2);
    int new_target_id = TARGET_POSITION2;
    int got_new_id = 0;
    // 再次获取当前位置二维码ID
    for (int try = 0; try < 3; ++try) {
        int len = UART5_ReceiveERRData(err_buffer, sizeof(err_buffer), 2000);
        if (len > 0) {
            qr_err = Pose_ParseQRData(err_buffer);
            if (qr_err.valid) {
                curr_id = qr_err.id;
                got_new_id = 1;
                break;
            }
        }
        DELAY_MS(500);
    }
    if (!got_new_id) {
        printf("无法获取当前位置二维码ID，流程中断\r\n");
        while (1) { DELAY_MS(1000); }
    }
    // 路径规划与逐步移动到新目标
    while (curr_id != new_target_id) {
        int path[9];
        int path_len = Map_GetFullPath(curr_id, new_target_id, path, 9);
        if (path_len < 2) {
            printf("无法规划到新目标或已到达\r\n");
            break;
        }
        int next_id = path[1];
        printf("从%d移动到%d\r\n", curr_id, next_id);
        // === 新增：移动前避障 ===
        while (HCSR04_IsObstacleDetected()) {
            printf("前方有障碍物，等待清除...\r\n");
            StopAllMotors();
            DELAY_MS(500);
        }
        MoveOneStep(curr_id, next_id);
        // === 新增：移动后再次避障检测 ===
        if (HCSR04_IsObstacleDetected()) {
            printf("移动后检测到障碍物，已停车，等待障碍物消失...\r\n");
            StopAllMotors();
            while (HCSR04_IsObstacleDetected()) {
                DELAY_MS(500);
            }
            printf("障碍物已清除，继续流程\r\n");
        }
        // 到达新二维码后，进行姿态矫正
        Pose_AdjustByK230UART5(2.0f, 2.0f, 3000);
        // 再次获取当前二维码ID（通过UART5）
        int got_new_id2 = 0;
        for (int try = 0; try < 3; ++try) {
            int len = UART5_ReceiveERRData(err_buffer, sizeof(err_buffer), 2000);
            if (len > 0) {
                qr_err = Pose_ParseQRData(err_buffer);
                if (qr_err.valid) {
                    curr_id = qr_err.id;
                    got_new_id2 = 1;
                    break;
                }
            }
            DELAY_MS(500);
        }
        if (!got_new_id2) {
            printf("无法获取新二维码ID，流程中断\r\n");
            break;
        }
    }
    // 到达新目标二维码后，最后一次姿态矫正
    printf("已到达新目标二维码%d，进行最终姿态矫正\r\n", new_target_id);
    Pose_AdjustByK230UART5(2.0f, 2.0f, 3000);
    StopAllMotors();
    printf("小车已停止\r\n");
    // 再次舵机往返运动
    printf("再次开始舵机往返运动演示\r\n");
    ServoDemo();
    printf("全部流程结束\r\n");
    while (1) {
        DELAY_MS(1000);
    }
}

/**
 * @brief 检查障碍物并停止
 */
void CheckObstacleAndStop(void)
{
    if (HCSR04_IsObstacleDetected()) {
        g_bObstacleDetected = true;
        StopAllMotors();
        printf("检测到障碍物，停止运动！\r\n");
    } else {
        g_bObstacleDetected = false;
    }
}
